#include "WPILib.h"
	const float SHOOTING_MOTOR_SPEED = 0.6;
	const float SHOOTING_MOTOR_DIFFERENCE=-0.2;
 
class RobotDemo : public SimpleRobot
{
	RobotDrive myRobot; // robot drive system
	Joystick joyStick; // drive
	Joystick secondPersonJoyStick; // shooter
	Victor shootingMotorFront; //shooter back
	Victor shootingMotorBack; //shooter front
	Victor loader;
	//Relay loader;
	float shootingMotorFrontSpeed;
	float shootingMotorBackSpeed;
	
	
public:
	RobotDemo(void):
		myRobot(1, 2),	// these must be initialized in the same order
		joyStick(1),		// as they are declared above.
		secondPersonJoyStick(2),
		shootingMotorFront(3),
		shootingMotorBack(4),
		loader(5)

	{
		myRobot.SetExpiration(0.1);
	}
	void Autonomous(void)
	{
		float shootingMotorFrontSpeed;
		float shootingMotorBackSpeed;
		//myRobot.SetSafetyEnabled(false);
		//myRobot.Drive(-0.5, 0.0); 	// drive forwards half speed
		//Wait(2.0); 				//    for 2 seconds
		//myRobot.Drive(0.0, 0.0); 	// stop robot
		shootingMotorFrontSpeed=SHOOTING_MOTOR_SPEED; // Need seperate routine to set motors
		shootingMotorBackSpeed=SHOOTING_MOTOR_SPEED-SHOOTING_MOTOR_DIFFERENCE; // To correct speeds
		shootingMotorBack.Set(shootingMotorBackSpeed);
		shootingMotorFront.Set(shootingMotorFrontSpeed);
		Wait(3.0);
		loader.Set(1.0);
		Wait(10.0);
		shootingMotorBack.Set(0.0);
		shootingMotorFront.Set(0.0);
		
	}

	/**
	 * Runs the motors with arcade steering. 
	 */
	void OperatorControl(void)
	{
//		float shootingMotorFrontSpeed_prev;
		float shootingMotorFrontSpeed;
//		float shootingMotorBackSpeed_prev;	
		float shootingMotorBackSpeed;
		//bool wasPressedInPreviousStep=false;
		myRobot.SetSafetyEnabled(true);
		while (IsOperatorControl())
		{
			
			shootingMotorFrontSpeed=SHOOTING_MOTOR_SPEED;
			/*shootingMotorFrontSpeed_prev = shootingMotorFrontSpeed;
			shootingMotorFrontSpeed = -joyStick.GetThrottle();			
			if (joyStick.GetRawButton(10)){
				if (!wasPressedInPreviousStep){SHOOTING_MOTOR_DIFFERENCE+=.01;
			wasPressedInPreviousStep=true;
			}}else{
			wasPressedInPreviousStep=false;	
			}
			if (joyStick.GetRawButton(11)){
				if (!wasPressedInPreviousStep){SHOOTING_MOTOR_DIFFERENCE-=.01;
			wasPressedInPreviousStep=true;
			}}else{
			wasPressedInPreviousStep=false;	
			}
			if (joyStick.GetRawButton(11)){SHOOTING_MOTOR_DIFFERENCE-=.01;}
			*/
			//shootingMotorBackSpeed_prev = shootingMotorBackSpeed;
			shootingMotorBackSpeed = shootingMotorFrontSpeed-SHOOTING_MOTOR_DIFFERENCE;
			//shootingMotorBackSpeed_prev = shootingMotorBackSpeed;
			//shootingMotorBackSpeed = -secondPersonJoyStick.GetThrottle();
			/*
			if (shootingMotorFrontSpeed != shootingMotorFrontSpeed_prev)// || shootingMotorBackSpeed != shootingMotorBackSpeed_prev)
			{
				printf("shootingMotorFront = %f shootingMotorBack = %f\n", shootingMotorFrontSpeed, shootingMotorBackSpeed);
			}
			*/
			
			myRobot.SetInvertedMotor(RobotDrive::kRearLeftMotor, true);
			myRobot.SetInvertedMotor(RobotDrive::kRearRightMotor, true);
			myRobot.ArcadeDrive(joyStick, true); // drive with arcade style (use right stick)
			if (joyStick.GetRawButton(2)||joyStick.GetRawButton(2))
			{
				//printf("button2\n");
				shootingMotorBack.Set(shootingMotorBackSpeed);
				shootingMotorFront.Set(shootingMotorFrontSpeed);
			} else {
				shootingMotorBack.Set(0.0);
				shootingMotorFront.Set(0.0);
			}
			
			//Code for shooter assuming spike relay
			//if (joyStick.GetTrigger())
			//{
			//loader.Set(Relay::kForward);
			//} //else {
			//loader.Set(Relay::kOff);
			//}
			//" assuming victor controller
			if (joyStick.GetTrigger()||secondPersonJoyStick.GetTrigger()){
				loader.Set(1.0);
			} else {
				loader.Set(0.0);
			}
			Wait(0.005);				// wait for a motor update time
		}
	}
	
	/**
	 * Runs during test mode
	 */
	void Test() {

	}
};

START_ROBOT_CLASS(RobotDemo);

